Advertisement
Guest User

Untitled

a guest
Jun 10th, 2018
94
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 8.45 KB | None | 0 0
  1. #include <Servo.h>
  2. #include <CheapStepper.h>
  3.  
  4. //define I/O ports
  5. #define inputSelector A5
  6. #define limitSwitch A4
  7. #define spindleRotation 2
  8. #define LED1 3
  9. #define LED2 4
  10. #define startSwitch 5
  11. #define stopSwitch 6
  12. #define spindleServo 7
  13. #define UN2003IN4 9
  14. #define UN2003IN3 10
  15. #define UN2003IN2 11
  16. #define UN2003IN1 12
  17. #define outputVoltage A1
  18. #define potentioMeter A2
  19.  
  20. //define stepper speed
  21. #define stepperNormalSpeed 2
  22. #define stepperFastSpeed 20
  23. #define dangerLowVoltage 230
  24. #define hysteresis 5
  25. #define stepperMaxTravel 245760
  26. #define holdOffDistance 3000
  27.  
  28. //define variables
  29. int currentPotPosition = 0;
  30. int currentVoltage = 0;
  31. int lowerHysteresis = 0;
  32. int higherHysteresis = 0;
  33. int startLocation = 0;
  34. int shortCircuitLocation = 0;
  35. bool startLocationSet = false;
  36. bool moveBackward = true;
  37. bool moveUp = false;
  38. bool moveDown = false;
  39. bool starT = false;
  40. bool stoP = false;
  41. int stepsLeft = 0;
  42. static unsigned int stateMachine = 0; // Initialize the state as "inactive"
  43.  
  44. //Setup servos
  45. Servo spindle;
  46. CheapStepper stepper (12,11,10,9);
  47.  
  48.  
  49.  
  50. // Simple macros to declare debounce input pins.. Saves time typing.
  51. #define DEBOUNCE_DECLARE(p) \
  52. bool p ## _state = 0; \
  53. int p ## _count = 0;
  54.  
  55. // macro to debounce a pin.
  56. #define debounceCount 10
  57. #define DEBOUNCE(p) do { \
  58. if(digitalRead(p) == HIGH) { \
  59. p ## _count++; \
  60. if(p ## _count >= debounceCount) { \
  61. p ## _state = 1; \
  62. } else { \
  63. p ## _state = 0; \
  64. } \
  65. } else { \
  66. p ## _count = 0; \
  67. p ## _state = 0; \
  68. } \
  69. } while(0) /* We use do{ }while(0) because this allows us to put a ";" behind the macro when using it. */
  70.  
  71. // Declare the variables we use to store the debounced state in.
  72. DEBOUNCE_DECLARE(inputSelector);
  73. DEBOUNCE_DECLARE(spindleRotation);
  74. DEBOUNCE_DECLARE(startSwitch);
  75. DEBOUNCE_DECLARE(stopSwitch);
  76. DEBOUNCE_DECLARE(limitSwitch);
  77.  
  78. void setup(){
  79.  
  80. // Configure I/O
  81. pinMode(inputSelector, INPUT);
  82. pinMode(limitSwitch, INPUT);
  83. pinMode(spindleRotation, INPUT);
  84. pinMode(LED1, OUTPUT);
  85. pinMode(LED2, OUTPUT);
  86. pinMode(startSwitch, INPUT);
  87. pinMode(stopSwitch, INPUT);
  88. pinMode(spindleServo, OUTPUT);
  89. pinMode(UN2003IN1, OUTPUT);
  90. pinMode(UN2003IN2, OUTPUT);
  91. pinMode(UN2003IN3, OUTPUT);
  92. pinMode(UN2003IN4, OUTPUT);
  93. pinMode(outputVoltage, INPUT);
  94. pinMode(potentioMeter, INPUT);
  95. spindle.attach(spindleServo); // Attach pulse output pin for spindle servo
  96. // Serial.begin(115200);
  97. }
  98.  
  99.  
  100.  
  101.  
  102.  
  103. void loop(){
  104. // Debounce our input pins.
  105. DEBOUNCE(stopSwitch);
  106. DEBOUNCE(startSwitch);
  107. DEBOUNCE(spindleRotation);
  108. DEBOUNCE(inputSelector);
  109. DEBOUNCE(limitSwitch);
  110. if(inputSelector_state == 1){
  111. if (startSwitch_state == 1 && stopSwitch_state == 0){
  112. moveUp = true;
  113. moveDown = false;
  114. } else if (stopSwitch_state == 1 && startSwitch_state == 0){
  115. moveUp = false;
  116. moveDown = true;
  117. } else {
  118. moveUp = false;
  119. moveDown = false;
  120. }
  121.  
  122. } else
  123. {
  124. moveUp = false;
  125. moveDown = false;
  126. if(startSwitch_state == 1 && stopSwitch_state == 0){
  127. starT = true;
  128. stoP = false;
  129. } else if (stopSwitch_state == 1 && startSwitch_state == 0){
  130. starT = false;
  131. stoP = true;
  132. }else {
  133. starT = false;
  134. stoP = false;
  135. }
  136. }
  137. stepsLeft = stepper.getStepsLeft();
  138.  
  139.  
  140.  
  141. switch (stateMachine){
  142. case 0: // Standby state.
  143. //Serial.println("Case 0, Standby");
  144. if(moveUp == 1){
  145. /*stepper.setRpm(stepperFastSpeed);
  146. moveBackward = true;
  147. stepper.newMove (moveBackward, 20);
  148. stepper.run(); */
  149. stepper.step(1);
  150. } else if(moveDown == 1){
  151. /*stepper.setRpm(stepperFastSpeed);
  152. moveBackward = false;
  153. stepper.newMove (moveBackward, 20);
  154. stepper.run();*/
  155. stepper.step(0);
  156. } else{
  157. stepper.stop();
  158. }
  159. if(starT == 1){ // Start switch activated
  160. digitalWrite(LED1, 1); // Turn on LED
  161. stepper.setRpm(stepperNormalSpeed); //Set stepper normal speed for regular runtime
  162. moveBackward = false; // Run forward
  163. stepper.newMoveTo (moveBackward, stepperMaxTravel); //number of degrees to move forward. (This is full the travel of the ballscrew)
  164. stateMachine = 1;
  165. }
  166. break;
  167. case 1: // Run state. The LED is on, wait for instructions to stop.
  168. //Serial.println("Case 1, RUN");
  169. //if(spindleRotation_state == 1) {
  170. //Serial.println("SpindleRotation_state = 1");
  171. // spindle.write(180); // Set spindle rotation at full speed (180=full speed clockwise, 90=stop, 0=full speed counter clockwise)
  172. //} else{
  173. //Serial.println("SpindleRotation_state = 0");
  174. // spindle.write(90); //stop spindle rotation
  175. //}
  176. currentVoltage = analogRead(outputVoltage); //reads the voltage number from the analog input
  177. currentPotPosition = analogRead(potentioMeter); //Reads the position of the potentiometer
  178. lowerHysteresis = (currentPotPosition - hysteresis); //Sets lower hysteresis
  179. higherHysteresis = (currentPotPosition + hysteresis); //Sets higher hysteresis
  180.  
  181. if(stoP == 1 or limitSwitch_state == 1){ // When this input is high, go to stop
  182. digitalWrite(LED1, 0); // Switch off LED.
  183. stepper.stop(); //stop stepper
  184. spindle.write(90); //stop spindle rotation
  185. stateMachine = 2;
  186. }
  187. if(higherHysteresis > currentVoltage > lowerHysteresis) { // hold it right there, thats the spot!
  188. stepper.stop();
  189. if(startLocationSet == true){
  190. startLocationSet = true;
  191. startLocation = stepper.getStep(); //Get the location of the first hysteresis (close to surface)
  192. }
  193. } else if(currentVoltage <= dangerLowVoltage) { //Detecting short circuit
  194. stateMachine = 3;
  195.  
  196. } else {
  197. stepper.newMoveTo (moveBackward, stepperMaxTravel); //number of degrees to move forward. (This is full the travel of the ballscrew)
  198. stepper.run();
  199. }
  200. break;
  201.  
  202. case 2: // Stop state
  203. //Serial.println("Case 2, STOP");
  204. moveBackward = true; //Reverse stepper direction
  205. stepper.setRpm(stepperFastSpeed); // Set fast step speed for retraction.
  206. stepper.newMoveTo(moveBackward, 0); //Set stepper location to start position
  207. if(stepsLeft == 0){
  208. stateMachine = 4;
  209. } else {
  210. stepper.run();
  211. }
  212. break;
  213.  
  214. case 3: // short circut
  215. //Serial.println("Case 3, Short circuit");
  216. stepper.stop(); //stop stepper
  217. shortCircuitLocation = stepper.getStep(); //Get the location of the short circuit
  218. moveBackward = true; //change direction of movement
  219. stepper.setRpm(stepperFastSpeed); //sets fast speed for stepper
  220. stepper.newMoveTo(moveBackward, startLocation); //makes stepper move to start position
  221. stateMachine = 5;
  222. break;
  223.  
  224. case 4: // Reset variables & return to standby
  225. //Serial.println("Case 4, Reset");
  226. //reset all variables
  227. stateMachine = 0;
  228. break;
  229.  
  230. case 5: // short circuit backwards movement
  231. //Serial.println("Case 5, Short circuit back");
  232. if(stepsLeft == 0){ //if movement is done
  233. stepper.stop(); //stop stepper
  234. moveBackward = false; //change direction of movement
  235. stepper.newMoveTo(moveBackward, shortCircuitLocation - holdOffDistance); //Move back close to the point of short circuit
  236. stateMachine = 6; //set stateMachine to short circuit forward movement
  237. } else {
  238. stepper.run(); //continue running stepper
  239. }
  240. break;
  241.  
  242. case 6: // short circuit forwards movement
  243. //Serial.println("Case 6, Short circuit forward");
  244. if(stepsLeft == 0){ //if movement is done
  245. moveBackward = false;
  246. stepper.newMoveTo(moveBackward, stepperMaxTravel);
  247. stateMachine = 1; //set stateMachine to running
  248. stepper.setRpm(stepperNormalSpeed);
  249. } else{
  250. stepper.run(); //Continue running stepper
  251. }
  252. break;
  253.  
  254. default: // Unknown state!
  255. //Serial.println("Case default, unknown state");
  256. stateMachine = 0; // Go to standby.
  257. break;
  258. }
  259. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement